#!/usr/bin/env python

from optparse import OptionParser
import gtk, gobject, pango
from datetime import datetime
import numpy as np

import pat.utils  as pu
import pat.frames as pfr

import rt_sim                 as sim
import pat.gui.scrolling_plot as gsp

class SimGui(gtk.Frame):
    """
    This is the graphical interface for controling the real-time simulation
    """
    def __init__(self, _app):
        self.sim = _app.sim
        _app.connect("update_slow", self.update)
        gtk.Frame.__init__(self, "Simulator")       
        hbox = gtk.HBox()
        self.add(hbox)
        self._label_time = gtk.Label('time: 0.00 s')
        hbox.pack_start(self._label_time)
        self._button_start_stop = gtk.Button(label="Start", stock=None)
        self._button_start_stop.connect("clicked", self.button_callback, "toggle")
        hbox.pack_start(self._button_start_stop)
        self._button_reset = gtk.Button(label="Reset", stock=None)
        hbox.pack_start(self._button_reset)
        self._button_reset.connect("clicked", self.button_callback, "reset")

    def button_callback(self, widget, data=None):
        if data == "toggle":
            self.sim.toggle_on_off()
            if self.sim.is_running():
                self._button_start_stop.set_label("Stop")
            else:
                self._button_start_stop.set_label("Start")
        elif data == "reset":
            self.sim.reset()
        
    def update(self, source):
        self._label_time.set_text("time: {:.2f} s".format(self.sim.get_time()))
        
        


class ApsGui(gtk.Frame):
    """
    This is the graphical interface for the autopilot
    """
    def __init__(self, _app):
        gtk.Frame.__init__(self, "Autopilot")

        self.ap = _app.sim.ap
        vbox = gtk.VBox()
        self.add(vbox)
        
        # mode selection
        hbox = gtk.HBox()
        vbox.pack_start(hbox, expand=False)
        hbox.pack_start(gtk.Label('Mode'), expand=False)
        combobox = gtk.combo_box_new_text()
        hbox.pack_start(combobox, expand=False)
        for m in self.ap.modes:
            combobox.append_text(m.name)
        combobox.set_active(_app.sim.ap.get_idx_selected_mode())
        combobox.connect("changed", self.mode_changed_cb)

        # reference selection
        hbox = gtk.HBox()
        vbox.pack_start(hbox, expand=False)
        hbox.pack_start(gtk.Label('Reference'), expand=False)
        self.combo_ref = gtk.combo_box_new_text()
        hbox.pack_start(self.combo_ref, expand=False)
        for m in self.ap.get_cur_mode().reference:
            self.combo_ref.append_text(m.name)  
            
        self.combo_ref.connect("changed", self.ref_changed_cb)
        
        # a text area to display parameters
        sw = gtk.ScrolledWindow()
        sw.set_policy(gtk.POLICY_AUTOMATIC, gtk.POLICY_AUTOMATIC)
        self.textview = gtk.TextView()
        self.textview.modify_font(pango.FontDescription("Courier 10"))
        textbuffer = self.textview.get_buffer()
        sw.add(self.textview)
        vbox.pack_start(sw)
        textbuffer.set_text(self.ap.properties())

        # a label for the output
        self.lab_U = gtk.Label("U: N/A")
        vbox.pack_start(self.lab_U, expand=False, fill=False)
        self.set_size_request(300, 200)
        _app.connect("update_slow", self.update)

    def update(self, _app):
        self.lab_U.set_text("U: {:s}".format(_app.sim.ap.get_U_str()))
 
    def mode_changed_cb(self, combobox):
        self.ap.set_cur_mode(combobox.get_active())
        textbuffer = self.textview.get_buffer()
        textbuffer.set_text(self.ap.properties())
        model = self.combo_ref.get_model()
        self.combo_ref.set_model(None)
        model.clear()
        for m in self.ap.get_cur_mode().reference:
            model.append([m.name])
        self.combo_ref.set_model(model)

            
    def ref_changed_cb(self, combobox):
        self.ap.set_ref(self.ap.get_cur_mode().reference[combobox.get_active()])
        

class DynamicGui(gtk.Frame):
    """
    This is the graphical interface for the dynamics
    Displays state and parameters
    """
    def __init__(self, _app):
        gtk.Frame.__init__(self, "Dynamics")       
        notebook = gtk.Notebook()
        notebook.set_tab_pos(gtk.POS_TOP)
        self.add(notebook)
        
        notebook.append_page(self.make_state_tab(_app.sim.dm), gtk.Label("State"))
        notebook.append_page(self.make_parameters_tab(_app.sim.dm), gtk.Label("Parameters"))

        _app.connect("update_slow", self.update)


    def make_state_tab(self, _dm):
        hbox = gtk.VBox(False, 0)
        vbox = gtk.VBox(False, 0)
        hbox.pack_start(vbox, expand=True, fill=True)
        self.lab_name = gtk.Label("Name: {:s}".format(_dm.name()))
        vbox.pack_start(self.lab_name, expand=False, fill=False)
        self.label_state = gtk.Label("State: N/A")
        vbox.pack_start(self.label_state, expand=False, fill=False)
        return hbox

    def make_parameters_tab(self, _dm):
        hbox = gtk.HBox()
        textview = gtk.TextView()
        textview.modify_font(pango.FontDescription("Courier 10"))
        textbuffer = textview.get_buffer()
        textbuffer.set_text(_dm.param().__str__())
        sw = gtk.ScrolledWindow()
        sw.add(textview)
        hbox.pack_start(sw, expand=True, fill=True)
        return hbox

    def update(self, _app):
        self.label_state.set_text("State: {:s}".format(_app.sim.dm.state_str()))


class VertDynScrollingPlot(gtk.Frame):
    
    def __init__(self, _app):
        self.sim = _app.sim
        gtk.Frame.__init__(self, "Longitudinal Dynamic")
        table = gtk.Table(5,3)
        self.add(table)
        self.plot_z = gsp.PlotGuy(title="$z$") 
        table.attach(self.plot_z, 0, 1, 0,1)
        self.plot_v = gsp.PlotGuy(title="$v$") 
        table.attach(self.plot_v, 1, 2, 0,1)
        self.plot_alpha = gsp.PlotGuy(title="$\\alpha$") 
        table.attach(self.plot_alpha, 0, 1, 1,2)
        self.plot_theta = gsp.PlotGuy(title="$\\theta$") 
        table.attach(self.plot_theta, 1, 2, 1,2)
        _app.connect("update_fast", self.update)
        _app.connect("update_slow", self.update_slow)
        self.cnt_update_plot = 0

    def update(self, _app):
        t = self.sim.get_time()
        dm = _app.sim.dm
        self.plot_z.add_data(t,  dm.X[dm.sv_z])
        self.plot_v.add_data(t,  dm.X[dm.sv_v])
        self.plot_alpha.add_data(t,  pu.deg_of_rad(dm.X[dm.sv_alpha]))
        self.plot_theta.add_data(t,  pu.deg_of_rad(dm.X[dm.sv_theta]))

    def update_slow(self, _dm):
        self.cnt_update_plot += 1
        j = self.cnt_update_plot%4
        if j == 0:   self.plot_z.update_plot()
        elif j == 1: self.plot_v.update_plot()
        elif j == 2: self.plot_alpha.update_plot()
        elif j == 3: self.plot_theta.update_plot()


class SixDOFfEuclidianEulerPlotSmall(gtk.Frame):
    def __init__(self, _app):
        gtk.Frame.__init__(self, "6DOF Dynamic")
        table = gtk.Table(4,3)
        self.plots = []
        self.add(table)
        p = gsp.PlotGuy(title="$x$", ncurve=2) 
        self.plots.append(p)
        table.attach(p, 0, 1, 0,1)
        p = gsp.PlotGuy(title="$y$") 
        self.plots.append(p)
        table.attach(p, 1, 2, 0,1)
        p = gsp.PlotGuy(title="$z$") 
        self.plots.append(p)
        table.attach(p, 2, 3, 0,1)

        p = gsp.PlotGuy(title="$\phi$") 
        self.plots.append(p)
        table.attach(p, 0, 1, 1,2)
        p = gsp.PlotGuy(title="$\\theta$") 
        self.plots.append(p)
        table.attach(p, 1, 2, 1,2)
        p = gsp.PlotGuy(title="$\psi$") 
        self.plots.append(p)
        table.attach(p, 2, 3, 1,2)

        _app.connect("update_fast", self.update)
        _app.connect("update_slow", self.update_slow)
        self.cnt = 0

    def update(self, _app):
        t = _app.sim.get_time()
        fmt = pfr.SixDOFfEuclidianEuler
        if _app.sim.ap.ref <> None:
            self.plots[0].add_data(t,  [_app.sim.X6EE[fmt.x], _app.sim.ap.refi[0]])
        else:
            self.plots[0].add_data(t,  _app.sim.X6EE[fmt.x])
        self.plots[1].add_data(t,  _app.sim.X6EE[fmt.y])
        self.plots[2].add_data(t,  _app.sim.X6EE[fmt.z])
        phi, theta, psi = pu.deg_of_rad(_app.sim.X6EE[fmt.phi:fmt.psi+1])
        self.plots[3].add_data(t,  phi)
        self.plots[4].add_data(t, theta)
        self.plots[5].add_data(t,  psi)
            

        
    def update_slow(self, _app):
        self.cnt+=1;
        i = self.cnt%6
        self.plots[i].update_plot()

class PlanarVtolPlot(gtk.Frame):
    def __init__(self, _app):
        gtk.Frame.__init__(self, "6DOF Dynamic")
        table = gtk.Table(2,2)
        self.plots = []
        self.add(table)
        def add_plot(i, j, ncurve, title, ylab):
            p = gsp.PlotGuy(ncurve=ncurve, title=title, ylab=ylab) 
            self.plots.append(p)
            table.attach(p, i, i+1, j,j+1)
        add_plot(0, 0, 1, "$x$", "m") 
        add_plot(1, 0, 1, "$z$", "m") 
        add_plot(0, 1, 1, "$\\theta$", "deg") 
        add_plot(1 ,1, 2, "$U$", "N") 
        _app.connect("update_fast", self.update)
        _app.connect("update_slow", self.update_slow)
        self.cnt = 0

    def update(self, _app):
        t = _app.sim.get_time()
        fmt = pfr.SixDOFfEuclidianEuler
        self.plots[0].add_data(t,  _app.sim.X6EE[fmt.x])
        self.plots[1].add_data(t,  _app.sim.X6EE[fmt.z])
        self.plots[2].add_data(t,  pu.deg_of_rad(_app.sim.X6EE[fmt.theta]))
        self.plots[3].add_data(t,  _app.sim.ap.U)

    def update_slow(self, _app):
        self.cnt+=1;
        i = self.cnt%4
        self.plots[i].update_plot()    



class MyApplication(gobject.GObject):
    __gsignals__ = { 
#        "update_fast" : (gobject.SIGNAL_RUN_LAST, gobject.TYPE_NONE,(int, np.ndarray, )),
        "update_fast" : (gobject.SIGNAL_RUN_LAST, gobject.TYPE_NONE,()),
        "update_slow" : (gobject.SIGNAL_RUN_LAST, gobject.TYPE_NONE,())
        } 
    def __init__(self, options):
        gobject.GObject.__init__(self)

        if options.planar_vtol and not options.scrolling_plot: options.scrolling_plot = 2
        
        self.sim = sim.Simulator(options)
        self.sim.connect("updated", self.sim_updated)

        self._window = gtk.Window(gtk.WINDOW_TOPLEVEL)
        self._window.connect('destroy', lambda wid: gtk.main_quit())
        self._window.connect('delete_event', lambda a1,a2:gtk.main_quit())
        self._window.set_title("PATSim")

        vbox = gtk.VBox()
        self._window.add(vbox)

        self._sgui = SimGui(self)
        vbox.pack_start(self._sgui, expand=False, fill=False)

        vpaned = gtk.VPaned()
        vbox.pack_start(vpaned, expand=True, fill=True)

        hpaned = gtk.HPaned()
        vpaned.add1(hpaned)

        self._dgui = DynamicGui(self)
        hpaned.add1(self._dgui)

        self._apgui = ApsGui(self)
        hpaned.add2(self._apgui)

        if options.scrolling_plot:
            scps = [VertDynScrollingPlot, SixDOFfEuclidianEulerPlotSmall, PlanarVtolPlot]
            self.scrolling_plot = scps[int(options.scrolling_plot)](self)
            vpaned.add2(self.scrolling_plot)
            self._window.set_size_request(1400, 1100)
        else:
            self._window.set_size_request(1400, 400)

        self._window.show_all()
        self.cnt = 0
        self.sim.reset()

    def sim_updated(self, source):
        self.cnt+=1
        if self.cnt% 10 == 0: 
            self.emit("update_slow")#, self._sim.get_time(), self._sim._dm.X)
        self.emit("update_fast")#, self._sim.get_time(), self._sim._dm.X)

    def main(self):
        gtk.main()


if __name__ == "__main__":
    np.set_printoptions(precision=5, suppress=False, linewidth=120)

    usage = "usage: %prog [options]"
    version = "%prog 1.0"
    parser = OptionParser(usage=usage, version=version)
    # scrolling plot
    parser.add_option("-p", "--sp", dest="scrolling_plot",
                      action="store",
                      help="scrolling plot")
    options = sim.parse_cmd_line(parser)
    print "options", options
    app = MyApplication(options)
    app.main()
